﻿using System;
using UnityEngine;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;

namespace Space2D
{
    public class AABBNode
    {
        public AABB aabbNode;
        public AABBTree tree;

        public AABBNode parent;
        public AABBNode left;
        public AABBNode right;

        public int depth;
        public int nodeNumber;

        public List<List<Vector2>> polys;
        public AABB AABBPoly
        {
            get
            {
                if (polys != null && polys.Count == 1)
                {
                    AABB aabb = AABB.AABBByPoly(polys[0]);
                    return aabb;
                }
                return null;
            }
        }

        public bool IsLeaf
        {
            get
            {
                bool isLeaf = polys != null;
                if (isLeaf && ((right != null) || (left != null)))
                {
                    throw new Exception("AABB叶子节点竟然有子节点！");
                }
                return isLeaf;
            }
        }

        #region AABBNode
        internal AABBNode(AABBTree tree)
        {
            this.tree = tree;
            polys = new List<List<Vector2>>();
            left = right = null;
            parent = null;
            nodeNumber = tree.nodeCount++;
        }
        internal AABBNode(AABBTree tree, List<List<Vector2>> pPolys) : this(tree, null, pPolys, 0) { }
        private AABBNode(AABBTree tree, AABBNode lparent, List<List<Vector2>> pPolys, int curdepth = 0)
        {
            this.tree = tree;
            nodeNumber = tree.nodeCount++;
            parent = lparent;
            depth = curdepth;
            if (tree.maxDepth < curdepth)
            {
                tree.maxDepth = curdepth;
            }
            if (pPolys == null || pPolys.Count < 1)
            {
                throw new Exception("AABBNode constructed with invalid paramaters");
            }
            if (pPolys.Count <= tree.LEAF_POLY_MAX_COUNT)
            {//检查是否在叶子结点，如果是，保存Poly并停止递归；同时存储叶子结点的min和max，更新父节点；
                left = null; //一旦到达叶子结点，必须将左右结点标记为null以表示这是终点
                right = null;
                polys = pPolys; //在叶子结点存储剩余Poly
                ComputeVolume();
                splitIfNecessary();
            }
            else
            {//如果有多于LEAF_POLY_MAX_COUNT个Poly，计算体积并分割
                polys = pPolys;
                ComputeVolume();
                SplitNode();
            }
        }
        #endregion

        internal void ComputeVolume()
        {
            aabbNode = AABB.AABBByPoly(polys[0]);
            for (int i = 1; i < polys.Count; i++)
            {
                AABB aabb = AABB.AABBByPoly(polys[i]);
                ExpandVolume(aabb.Min, aabb.Max);
            }
        }
        private void ExpandVolume(Vector2 otherMin, Vector2 otherMax)
        {
            bool expanded = false;
            Vector2 min = aabbNode.Min;
            Vector2 max = aabbNode.Max;
            if (otherMin.x < min.x) { min.x = otherMin.x; expanded = true; }
            if (otherMin.y < min.y) { min.y = otherMin.y; expanded = true; }
            if (otherMax.x > max.x) { max.x = otherMax.x; expanded = true; }
            if (otherMax.y > max.y) { max.y = otherMax.y; expanded = true; }
            aabbNode.Min = min;
            aabbNode.Max = max;
            if (expanded && parent != null)
            {
                parent.ChildExpanded(this);
            }
        }

        internal float SAofList(List<List<Vector2>> pPolys)
        {
            AABB aabb = AABB.AABBByPoly(pPolys[0]);
            pPolys.ToList().GetRange(1, pPolys.Count - 1).ForEach(pPoly =>
            {
                AABB aabbNew = AABB.AABBByPoly(pPoly);
                aabb.ExpandBy(aabbNew);
            });
            return SA(aabb);
        }
        internal static float SA(AABB aabb)
        {
            float x_size = aabb.Max.x - aabb.Min.x;
            float y_size = aabb.Max.y - aabb.Min.y;
            return 2.0f * x_size * y_size;
        }

        [DebuggerBrowsable(DebuggerBrowsableState.Never)]
        private static List<Axis2D> eachAxis
        {
            get
            {
                return new List<Axis2D>((Axis2D[])Enum.GetValues(typeof(Axis2D)));
            }
        }

        internal void SplitNode()
        {//2.决定分割哪个轴，排序
            List<List<Vector2>> splitlist = polys;
            int center = splitlist.Count / 2; //拿到中间的Poly的索引
            SplitAxisOpt bestSplit = eachAxis.Min((axis) =>
            {
                var orderedlist = new List<List<Vector2>>(splitlist);
                switch (axis)
                {
                    case Axis2D.X:
                        orderedlist.Sort(delegate (List<Vector2> poly1, List<Vector2> poly2)
                        {
                            AABB aabb1 = AABB.AABBByPoly(poly1);
                            AABB aabb2 = AABB.AABBByPoly(poly2);
                            return aabb1.Center.x.CompareTo(aabb2.Center.x);
                        });
                        break;
                    case Axis2D.Y:
                        orderedlist.Sort(delegate (List<Vector2> poly1, List<Vector2> poly2)
                        {
                            AABB aabb1 = AABB.AABBByPoly(poly1);
                            AABB aabb2 = AABB.AABBByPoly(poly2);
                            return aabb1.Center.y.CompareTo(aabb2.Center.y);
                        });
                        break;
                    default:
                        throw new NotImplementedException("unknown split axis: " + axis.ToString());
                }
                var left_s = orderedlist.GetRange(0, center);
                var right_s = orderedlist.GetRange(center, splitlist.Count - center);
                float SAH = SAofList(left_s) * left_s.Count + SAofList(right_s) * right_s.Count;
                return new SplitAxisOpt(SAH, axis, left_s, right_s);
            });
            //执行分割
            polys = null;
            left = new AABBNode(tree, this, bestSplit.left, depth + 1); // Split the Hierarchy to the left
            right = new AABBNode(tree, this, bestSplit.right, depth + 1); // Split the Hierarchy to the right                            
        }

        internal void splitIfNecessary()
        {
            if (polys.Count > tree.LEAF_POLY_MAX_COUNT)
            {
                SplitNode();
            }
        }

        internal void ChildExpanded(AABBNode child)
        {
            bool expanded = false;

            Vector2 min = aabbNode.Min;
            Vector2 max = aabbNode.Max;
            if (child.aabbNode.Min.x < aabbNode.Min.x)
            {
                min.x = child.aabbNode.Min.x; expanded = true;
            }
            if (child.aabbNode.Max.x > aabbNode.Max.x)
            {
                max.x = child.aabbNode.Max.x; expanded = true;
            }
            if (child.aabbNode.Min.y < aabbNode.Min.y)
            {
                min.y = child.aabbNode.Min.y; expanded = true;
            }
            if (child.aabbNode.Max.y > aabbNode.Max.y)
            {
                max.y = child.aabbNode.Max.y; expanded = true;
            }
            aabbNode.Min = min;
            aabbNode.Max = max;
            if (expanded && parent != null)
            {
                parent.ChildExpanded(this);
            }
        }

        public override string ToString()
        {
            return string.Format($"AABBNode{typeof(List<Vector2>)}:{nodeNumber}");
        }
    }

    internal class SplitAxisOpt : IComparable<SplitAxisOpt>
    {  // split Axis option
        public float SAH;
        public Axis2D axis;
        public List<List<Vector2>> left, right;
        internal SplitAxisOpt(float SAH, Axis2D axis, List<List<Vector2>> left, List<List<Vector2>> right)
        {
            this.SAH = SAH;
            this.axis = axis;
            this.left = left;
            this.right = right;
        }
        public int CompareTo(SplitAxisOpt other)
        {
            return SAH.CompareTo(other.SAH);
        }
    }
}